#include "QHsm.hpp"
#include "QAssert.hpp"

/// helper macro to trigger internal event in an HSM
#define QEP_TRIG_(state_, sig_) ((*(state_))(this, QEP_reservedEvt_[sig_]))

/// helper macro to trigger entry action in an HSM
#define QEP_ENTER_(state_)                                 \
  do {                                                     \
    if (QEP_TRIG_(state_, Q_ENTRY_SIG) == Q_RET_HANDLED) { \
    }                                                      \
  } while (false)

#define QEP_EXIT_(state_)                                 \
  do {                                                    \
    if (QEP_TRIG_(state_, Q_EXIT_SIG) == Q_RET_HANDLED) { \
    }                                                     \
  } while (false)

namespace QP {

Q_DEFINE_THIS_MODULE("QHsm")

QSignal const QEP_EMPTY_SIG_ = static_cast<QSignal>(0);

static std::shared_ptr<const QEvt> QEP_reservedEvt_[4] = {
    std::make_shared<QEvt>(static_cast<QSignal>(0)),
    std::make_shared<QEvt>(static_cast<QSignal>(1)),
    std::make_shared<QEvt>(static_cast<QSignal>(2)),
    std::make_shared<QEvt>(static_cast<QSignal>(3))};

QHsm::QHsm(QStateHandler const initial) {
  m_state = Q_STATE_CAST(&top);
  m_temp = initial;
}

void QHsm::init() { init(QEP_reservedEvt_[0]); }

void QHsm::init(const std::shared_ptr<const QEvt>& me) {
  QStateHandler t = m_state;
  Q_REQUIRE((m_temp != Q_STATE_CAST(0))           // ctor must be executed
            && (t == Q_STATE_CAST(&QHsm::top)));  // initial tran. NOT taken

  // execute the top-most initial transition
  QState r = (*m_temp)(this, me);
  // the top-most initial transition must be taken
  Q_ASSERT_ID(210, r == Q_RET_TRAN);

  do {
    QStateHandler path[MAX_NEST_DEPTH_];           // tran entry path array
    int_fast8_t ip = static_cast<int_fast8_t>(0);  // tran entry path index

    path[0] = m_temp;
    (void)QEP_TRIG_(m_temp, QEP_EMPTY_SIG_);
    while (m_temp != t) {
      ++ip;
      path[ip] = m_temp;
      (void)QEP_TRIG_(m_temp, QEP_EMPTY_SIG_);
    }
    m_temp = path[0];

    // retrace the entry path in reverse (desired) order...
    do {
      QEP_ENTER_(path[ip]);  // enter path[ip]
      --ip;
    } while (ip >= static_cast<int_fast8_t>(0));
    t = path[0];                   // current state becomes the new source
    r = QEP_TRIG_(t, Q_INIT_SIG);  // execute initial transition
  } while (r == Q_RET_TRAN);
  m_state = t;  // change the current active state
  m_temp = t;   // mark the configuration as stable
}

void QHsm::dispatch(const std::shared_ptr<const QEvt>& e) {
  QStateHandler t = m_state;
  /// @pre the current state must be initialized and
  /// the state configuration must be stable
  Q_REQUIRE_ID(400, (t != nullptr) && (t == m_temp));
  QStateHandler s;
  QState r;

  do {  // process the event hierarchically...
    s = m_temp;
    r = (*s)(this, e);  // invoke state handler s

    if (r == Q_RET_UNHANDLED) {          // unhandled due to a guard?
      r = QEP_TRIG_(s, QEP_EMPTY_SIG_);  // find superstate of s
    }
  } while (r == Q_RET_SUPER);
  // transition taken?
  if (r >= Q_RET_TRAN) {
    QStateHandler path[MAX_NEST_DEPTH_];

    path[0] = m_temp;  // save the target of the transition
    path[1] = t;
    path[2] = s;

    // exit current state to transition source s...
    for (; t != s; t = m_temp) {
      // exit handled?
      if (QEP_TRIG_(t, Q_EXIT_SIG) == Q_RET_HANDLED) {
        (void)QEP_TRIG_(t, QEP_EMPTY_SIG_);  // find superstate of t
      }
    }

    int_fast8_t ip = hsm_tran(path);  // take the HSM transition
    // retrace the entry path in reverse (desired) order...
    for (; ip >= static_cast<int_fast8_t>(0); --ip) {
      QEP_ENTER_(path[ip]);  // enter path[ip]
    }
    t = path[0];  // stick the target into register
    m_temp = t;   // update the next state

    // drill into the target hierarchy...
    while (QEP_TRIG_(t, Q_INIT_SIG) == Q_RET_TRAN) {
      ip = static_cast<int_fast8_t>(0);
      path[0] = m_temp;

      (void)QEP_TRIG_(m_temp, QEP_EMPTY_SIG_);  // find superstate

      while (m_temp != t) {
        ++ip;
        path[ip] = m_temp;
        (void)QEP_TRIG_(m_temp, QEP_EMPTY_SIG_);  // find superstate
      }
      m_temp = path[0];
      // entry path must not overflow
      Q_ASSERT_ID(410, ip < MAX_NEST_DEPTH_);
      // retrace the entry path in reverse (correct) order...
      do {
        QEP_ENTER_(path[ip]);  // enter path[ip]
        --ip;
      } while (ip >= static_cast<int_fast8_t>(0));
      t = path[0];
    }
  }
  m_state = t;  // change the current active state
  m_temp = t;   // mark the configuration as stable
}

int_fast8_t QHsm::hsm_tran(QStateHandler (&path)[MAX_NEST_DEPTH_]) {
  // transition entry path index
  int_fast8_t ip = static_cast<int_fast8_t>(-1);
  int_fast8_t iq;  // helper transition entry path index
  QStateHandler t = path[0];
  QStateHandler const s = path[2];
  QState r;
  // (a) check source==target (transition to self)
  if (s == t) {
    QEP_EXIT_(s);                      // exit the source
    ip = static_cast<int_fast8_t>(0);  // cause entering the target
  } else {
    (void)QEP_TRIG_(t, QEP_EMPTY_SIG_);  // superstate of target
    t = m_temp;

    // (b) check source==target->super
    if (s == t) {
      ip = static_cast<int_fast8_t>(0);  // cause entering the target
    } else {
      (void)QEP_TRIG_(s, QEP_EMPTY_SIG_);  // superstate of src

      // (c) check source->super==target->super
      if (m_temp == t) {
        QEP_EXIT_(s);                      // exit the source
        ip = static_cast<int_fast8_t>(0);  // cause entering the target
      } else {
        // (d) check source->super==target
        if (m_temp == path[0]) {
          QEP_EXIT_(s);  // exit the source
        } else {
          // (e) check rest of source==target->super->super..
          // and store the entry path along the way

          // indicate that the LCA was not found
          iq = static_cast<int_fast8_t>(0);

          // enter target and its superstate
          ip = static_cast<int_fast8_t>(1);
          path[1] = t;  // save the superstate of target
          t = m_temp;   // save source->super

          // find target->super->super
          r = QEP_TRIG_(path[1], QEP_EMPTY_SIG_);
          while (r == Q_RET_SUPER) {
            ++ip;
            path[ip] = m_temp;  // store the entry path
            if (m_temp == s) {  // is it the source?
              // indicate that the LCA was found
              iq = static_cast<int_fast8_t>(1);
              // entry path must not overflow
              Q_ASSERT_ID(510, ip < MAX_NEST_DEPTH_);
              --ip;               // do not enter the source
              r = Q_RET_HANDLED;  // terminate the loop
            }
            // it is not the source, keep going up
            else {
              r = QEP_TRIG_(m_temp, QEP_EMPTY_SIG_);
            }
          }

          // LCA found yet?
          if (iq == static_cast<int_fast8_t>(0)) {
            // entry path must not overflow
            Q_ASSERT_ID(520, ip < MAX_NEST_DEPTH_);
            QEP_EXIT_(s);  // exit the source

            // (f) check the rest of source->super
            //                  == target->super->super...
            //
            iq = ip;
            r = Q_RET_IGNORED;  // indicate LCA NOT found
            do {
              // is this the LCA?
              if (t == path[iq]) {
                r = Q_RET_HANDLED;  // indicate LCA found
                // do not enter LCA
                ip = static_cast<int_fast8_t>(iq - static_cast<int_fast8_t>(1));
                // cause termination of the loop
                iq = static_cast<int_fast8_t>(-1);
              } else {
                --iq;  // try lower superstate of target
              }
            } while (iq >= static_cast<int_fast8_t>(0));

            // LCA not found yet?
            if (r != Q_RET_HANDLED) {
              // (g) check each source->super->...
              // for each target->super...
              //
              r = Q_RET_IGNORED;  // keep looping
              do {
                // exit t unhandled?
                if (QEP_TRIG_(t, Q_EXIT_SIG) == Q_RET_HANDLED) {
                  (void)QEP_TRIG_(t, QEP_EMPTY_SIG_);
                }
                t = m_temp;  //  set to super of t
                iq = ip;
                do {
                  // is this LCA?
                  if (t == path[iq]) {
                    // do not enter LCA
                    ip = static_cast<int_fast8_t>(iq -
                                                  static_cast<int_fast8_t>(1));
                    // break out of inner loop
                    iq = static_cast<int_fast8_t>(-1);
                    r = Q_RET_HANDLED;  // break outer loop
                  } else {
                    --iq;
                  }
                } while (iq >= static_cast<int_fast8_t>(0));
              } while (r != Q_RET_HANDLED);
            }
          }
        }
      }
    }
  }
  return ip;
}

bool QHsm::isIn(QStateHandler const s) {
  Q_REQUIRE(m_temp == m_state);  // state configuration must be stable
  bool inState = false;          // assume that this HSM is not in 'state'
  QState r;
  do {
    if (m_temp == s) {    // do the states match?
      inState = true;     // match found, return TRUE
      r = Q_RET_IGNORED;  // break out of the loop
    } else {
      r = QEP_TRIG_(m_temp, QEP_EMPTY_SIG_);
    }
  } while (r != Q_RET_IGNORED);  // QHsm_top state not reached
  m_temp = m_state;              // restore the stable state configuration
  return inState;                // return the status
}

QState QHsm::top(void* const pSelf, const std::shared_ptr<const QEvt>& pEvt) {
  return Q_RET_IGNORED;
}
}
